#include <physics/World.h>
#include <physics/NodeFactory.h>
#include <physics/JointFactory.h>
#include <physics/UnknownJoint.h>

#include <Box2D/Dynamics/b2World.h>
#include <Box2D/common/b2math.h>

#include <math/Vector2.h>
#include <Resource/Manager.h>
#include <lib3d/Node/Node.h>

#include <physics/Box2DDebugRenderer.h>
#include <physics/PickCallback.h>

namespace physics
{

World::World()
	: b2world(new b2World(b2Vec2(0.0f, -1.0f), true))
	, debugRenderer(NULL)
	, velocityIterations(8)
	, positionIterations(3)
	, pixelsInMeter(30.0f)
{
}

World::~World()
{
	while(joints.size())
		RemoveJoint(joints[0]);

	while(nodes.size())
		RemoveNode(nodes[0]);

	if(debugRenderer != NULL)
		delete debugRenderer;
}
	
void World::SetDebugRenderer(DebugRenderer * renderer)
{
	// save renderer
	debugRenderer = renderer;
	
	// send renderer to Box2D
	b2world->SetDebugDraw(static_cast<Box2DDebugRenderer*>(debugRenderer));
}
	
void World::RenderToList(lib3d::hard::Hardware * hardware, lib3d::hard::RenderList * renderList)
{
	if(debugRenderer == NULL)
		return;
	
	// clear old debug data
	debugRenderer->Clear();
	
	// preccess all debug data
	b2world->DrawDebugData();
	
	// draw it
	debugRenderer->RenderToList(hardware, renderList);
}

bool World::on_node(xml::Provider * prov, const std::string & name)
{
	Node *node = NodeFactory::GetInstance()->Create(Atom(name), this);
	if(node)
		return prov->parse_object(node->CreateLoader().get());

	if(name == "Joint")
	{
		UnknownJoint *joint = new UnknownJoint(this);
		if(!prov->parse_object(joint->GetLoader()))
			return false;

		RemoveJoint(joint);
		return true;
	}

	Joint *joint = JointFactory::GetInstance()->Create(Atom(name), this);
	if(joint)
		return prov->parse_object(joint->GetLoader());

	return false;
}

bool World::on_attribute(xml::Provider * prov, const std::string & name, const std::string & value)
{
	if(name == "Gravity")
	{
		math::Vector2 <float> v;
		if(!converter::convert(value, &v))
			return false;

		b2world->SetGravity(b2Vec2(v.x, v.y));
		return true;
	}
	
	if(name == "PixelsInMeter")
		return converter::convert(value, &pixelsInMeter);
	
	if(name == "VelocityIterations")
		return converter::convert(value, &velocityIterations);
	
	if(name == "PositionIterations")
		return converter::convert(value, &positionIterations);

	return false;
}

bool World::on_end_node(xml::Provider *prov)
{
	for(std::vector <Joint*>::iterator it = joints.begin(); it != joints.end(); it++)
		if(!(*it)->Setup())
			return false;

	return true;
}

World* World::CreateFromResource(const filesystem::String &fileName)
{
	std::auto_ptr<resource::Resource> gameResource(resource::Manager::create_resource(fileName));
	
	World *world = new World();

	if (!xml::StreamResource(gameResource.get()).load(&xml::Root(world->GetLoader(), "World")))
		THROW(GluckBadResource(__FUNCTION__, "Failed to load physics world from resource", fileName));

	return world;
}

void World::SetRootNode(lib3d::node::Node *node)
{
	sceneNode = node;
	for(size_t i = 0; i < nodes.size(); i++)
		nodes[i]->SetRootNode(node);
}

void World::life_cycle(double deltaTime)
{
	for(size_t i = 0; i < nodes.size(); i++)
		nodes[i]->UpdateKinematic();
	
	for(size_t i = 0; i < nodes.size(); i++)
		nodes[i]->OnUpdate((float) deltaTime);

	b2world->Step((float) deltaTime, velocityIterations, positionIterations);
	
	for(size_t i = 0; i < nodes.size(); i++)
		nodes[i]->Update();
}
	
Node * World::FindNode(const Atom & name)
{
	for(size_t i = 0; i < nodes.size(); i++)
	{
		if(nodes[i]->GetName() == name)
			return nodes[i];
	}
	
	return NULL;
}

void World::AddNode(Node *node)
{
	nodes.push_back(node);
}

void World::RemoveNode(Node *node, bool deleteNode)
{
	std::vector <Node*>::iterator itr = std::find(nodes.begin(), nodes.end(), node);
	if(itr != nodes.end())
	{
		if(deleteNode)
			delete *itr;
		else
			nodes.erase(itr);
	}
}

Joint * World::FindJoint(const Atom & name)
{
	for(size_t i = 0; i < nodes.size(); i++)
	{
		if(joints[i]->GetName() == name)
			return joints[i];
	}

	return NULL;
}

void World::AddJoint(Joint *joint)
{
	joints.push_back(joint);
}

void World::RemoveJoint(Joint *joint, bool deleteJoint)
{
	std::vector <Joint*>::iterator itr = std::find(joints.begin(), joints.end(), joint);
	if(itr != joints.end())
	{
		if(deleteJoint)
			delete *itr;
		else
			joints.erase(itr);
	}
}
	
Node* World::Pick(const Vector2 &pickPoint)
{
	b2AABB aabb;
	aabb.lowerBound = b2Vec2(pickPoint.x / pixelsInMeter, pickPoint.y / pixelsInMeter) - b2Vec2(0.001f, 0.001f);
	aabb.upperBound = b2Vec2(pickPoint.x / pixelsInMeter, pickPoint.y / pixelsInMeter) + b2Vec2(0.001f, 0.001f);
	
	PickCallback callback;
	b2world->QueryAABB(&callback, aabb);
	
	return callback.getNode();
}

}  // namespace physics